/**
  ******************************************************************************
  * @file           : rudder_wheel_kinematic_drv.h
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/23
  ******************************************************************************
  */



#ifndef SENTRY_CHASSIS_RUDDER_WHEEL_KINEMATIC_DRV_H
#define SENTRY_CHASSIS_RUDDER_WHEEL_KINEMATIC_DRV_H

#include "drv_can_dr16.h"
#include "arm_math.h"
#include "cmsis_os.h"
#include "math.h"
#include "task_gyro.h"
#include "filtering_drv.h"
#include "chassis_motor_task.h"


const float SLOPE_SCALING_FACTOR = 80.f;
const float GYRO_FILTERED_ALPHA = 0.001;


struct motion_parameter{
    float vx;
    float vy;
    float omega; //旋转线速度，非角速度
};

struct rudder_round{
    float left_front_rudder;
    float left_back_rudder;
    float right_front_rudder;
    float right_back_rudder;
};

struct wheel_speed{
    float left_front_wheel;
    float left_back_wheel;
    float right_front_wheel;
    float right_back_wheel;
};


class rudder_wheel{

public:
    rudder_wheel(float speed_max_ = 1.0, float omega_max_ = 1.0f, float rudder2center_x_ = 190, float rudder2center_y_ = 190);
    void get_motion_parameter(float vx, float vy, float omega);
    void rudder_wheel_cal();

private:
    motion_parameter motionParameter;
    float speed_max; //归一化，轮子的最大速度
    float omega_max; //归一化，车体旋转速度
    //采用x朝左，y朝上坐标系，非机器人坐标系
    float rudder2center_x;   //车体中心距离轮子的距离x
    float rudder2center_y;   //车体中心距离轮子的距离y
    float alpha; //tan(alpha) = y/x     弧度
    uint8_t is_self_lock;           //自锁标志位

public:
    rudder_round rudderRound;
    rudder_round rudderRound_last;
    wheel_speed wheelSpeed;

};



#ifdef __cplusplus
extern "C" {
#endif
//C

#ifdef __cplusplus
}
#endif
//C++

#endif //SENTRY_CHASSIS_RUDDER_WHEEL_KINEMATIC_DRV_H
